#include "MyRobot.h"
#include "RobotUtils.h"
#include "VisualData.h"
#include "VideoGrabber.h"

using namespace std;
using namespace cv;

//	MODALITA DI ESECUZIONE
#define MOTION_AND_VISION

int main(int argc, char **argv) {

	//	messaggi di log su file
	ArLog::init(ArLog::File, ArLog::Normal,"log/aria-log.txt");

	//	TEST DELLA SOLA PARTE DI VISIONE
	//	
#ifdef VISION
	VisualData visualData;
	//	timer utilizzato per scandire le correzioni
	ArTime timer;
	timer.setToNow();
	ArPose globalPose;
	bool markerFound;

	while(true)
	{
		if(timer.mSecSince() > 1000)
		{
			markerFound = visualData.getGlobalPose(globalPose);

			if(!markerFound){
				ArLog::log(ArLog::Normal, "Main: nessun marker trovato");
			} else {
				cout << "X: " << globalPose.getX() << ", Y: " << globalPose.getY() << ", Th: " << globalPose.getTh() << endl;
			}
			//	sia che abbia trovato il marker oppure no, resetto il timer
			timer.setToNow();
		}

		//	in ogni caso aspetto 100 ms prima della prossima iterazione
		ArUtil::sleep(100);
	}
#endif
#ifdef MOTION_AND_VISION
	//	TEST CON MOTO
	//	

	Aria::init();
	MyRobot robot;

	ArArgumentParser parser(&argc, argv);
	parser.loadDefaultArguments();
	ArRobotConnector simpleConnector(&parser,&robot);
	ArSonarDevice sonar;
	ArAnalogGyro gyro(&robot);
	robot.addRangeDevice(&sonar);
	robot.enableSonar();

	// Collision avoidance actions at higher priority
	ArActionLimiterForwards limiterAction("speed limiter near", 300, 600, 250);
	ArActionLimiterForwards limiterFarAction("speed limiter far", 300, 1100, 400);
	ArActionLimiterTableSensor tableLimiterAction;
	robot.addAction(&tableLimiterAction, 100);
	robot.addAction(&limiterAction, 95);
	robot.addAction(&limiterFarAction, 90);

	// Stop action at lower priority, so the robot stops if it has no goal
	ArActionStop stopAction("stop");
	robot.addAction(&stopAction, 40);

	//	Make a key handler, so that escape will shut down the program
	//	cleanly
	ArKeyHandler keyHandler;
	Aria::setKeyHandler(&keyHandler);
	robot.attachKeyHandler(&keyHandler);
	printf("You may press escape to exit\n");

	// Parse all command line arguments
	if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed()) {
		Aria::logOptions();
		exit(1);
	}

	// Connect to the robot
	if (!simpleConnector.connectRobot(&robot)) {
		printf("Could not connect to robot... exiting\n");
		Aria::exit(1);
	}
	robot.runAsync(true);

	// turn on the motors, turn off amigobot sounds
	robot.enableMotors();
	robot.comInt(ArCommands::SOUNDTOG, 0);

	robot.setRotVelMax(30);
	robot.setTransVelMax(250);

	//	intervallo di correzione della posizione in funzione dei dati visuali
	int CORRECTION_INTERVAL;
	vector<ArPose> goals;

	RobotUtils::readCorrectionInterval(CORRECTION_INTERVAL);
	RobotUtils::readGoals(goals);

	VideoGrabber vg;
	vg.start();
	robot.reachGoalsAndCorrectPose(goals,CORRECTION_INTERVAL);

	vg.stop();
	Aria::shutdown();
#endif

	return 0;
}
